#include "display.h"
#include "fdecl.h"

void draw_cube(double size)
{
  move(-size, -size, -size);
  draw( size, -size, -size);
  draw( size,  size, -size);
  draw(-size,  size, -size);
  draw(-size, -size, -size);

  draw(-size, -size,  size);
  draw( size, -size,  size);
  draw( size, -size, -size);
  
  move(-size, -size,  size);
  draw(-size,  size,  size);
  draw( size,  size,  size);
  draw( size, -size,  size);

  move( size,  size,  size);
  draw( size,  size, -size);

  move(-size,  size,  size);
  draw(-size,  size, -size);
}


void draw_xyz_axes(double size)
{
  int current_color = getcolor();
  color(RED);
  move(0.0, 0.0, 0.0); draw(size, 0.0, 0.0);
  color(GREEN);
  move(0.0, 0.0, 0.0); draw(0.0, size, 0.0);
  color(BLUE);
  move(0.0, 0.0, 0.0); draw(0.0, 0.0, size);
  color(current_color);
}


void draw_overlay_xyz_axes(double size)
{
  int current_color = getcolor();
  color(overlay_color(RED));
  move(0.0, 0.0, 0.0); draw(size, 0.0, 0.0);
  color(overlay_color(GREEN));
  move(0.0, 0.0, 0.0); draw(0.0, size, 0.0);
  color(overlay_color(BLUE));
  move(0.0, 0.0, 0.0); draw(0.0, 0.0, size);
  color(current_color);
}


void draw_vector
  (vector *v, vector *p, double scale, int vector_color)
{
  vector p1;
  int current_color = getcolor();
  
  vscale(scale, *v, p1);
  vinc(*p, p1);
  color(vector_color);
  bgnline(); v3d((double *)p); v3d((double *)&p1); endline();
  color(current_color);
}


void draw_line_between_points
  (vector *p1, vector *p2, int line_color)
{
  int current_color = getcolor();
  
  color(line_color);
  bgnline(); v3d((double *)p1); v3d((double *)p2); endline();
  color(current_color);
}


void display_range_data(range_datapoint *data, int num_data_points)
{
  int i, k;
  range_datapoint *p;

  bgnpoint();
  for (i = 0, p = data, k = 0; i < num_data_points; i++) {
      if (k == 255) {endpoint(); bgnpoint(); k = 0;}
      v3d((double *)&(p++)->position);
    }
  endpoint();
}
     
void display_range_data_cube(range_datapoint *data, int num_data_points)
{
 int i;
 range_datapoint *p;

 for (i = 0, p = data; i < num_data_points; i++)
    draw_cube_p((p++)->position, 0.016);
}

void draw_cube_p(vector p, double size)
{
  move(-size+p.x, -size+p.y, -size+p.z);
  draw( size+p.x, -size+p.y, -size+p.z);
  draw( size+p.x,  size+p.y, -size+p.z);
  draw(-size+p.x,  size+p.y, -size+p.z);
  draw(-size+p.x, -size+p.y, -size+p.z);

  draw(-size+p.x, -size+p.y,  size+p.z);
  draw( size+p.x, -size+p.y,  size+p.z);
  draw( size+p.x, -size+p.y, -size+p.z);
  
  move(-size+p.x, -size+p.y,  size+p.z);
  draw(-size+p.x,  size+p.y,  size+p.z);
  draw( size+p.x,  size+p.y,  size+p.z);
  draw( size+p.x, -size+p.y,  size+p.z);

  move( size+p.x,  size+p.y,  size+p.z);
  draw( size+p.x,  size+p.y, -size+p.z);

  move(-size+p.x,  size+p.y,  size+p.z);
  draw(-size+p.x,  size+p.y, -size+p.z);
}


void display_range_data_hybroids(hybroid *hybroids[], int numhybroids)
{
  int i;

  for(i = 0; i < numhybroids; i++) {
/*used in case of occluded data*/
/*    if(i != 2)*/
  display_range_data_cube(hybroids[i]->range_data, hybroids[i]->numdatapoints);
/*
    display_range_data(hybroids[i]->range_data, hybroids[i]->numdatapoints);
*/
  }
}
      
/*

void display_range_data(range_datapoint *data, int num_data_points)
{
  int i, k;
  vector *p;

  bgnpoint();
  for (i = 0, p = data, k = 0; i < num_data_points; i++) {
      if (k == 255) {endpoint(); bgnpoint(); k = 0;}
      v3d((double *)p++);
    }
  endpoint();
}
     
*/    

void display_range_data_array(unsigned char *array, int xsize, int ysize, 
			double xscale, double yscale, double zscale)
{
  unsigned char *p;
  int i, j, k;
  double x, y, z;
  double point[3];

  bgnpoint();
  for (j = 0, p = array, k = 0; j < ysize; j++) {
    for (i = 0; i < xsize; i++, p++, k++) {
      if (k == 255) {endpoint(); bgnpoint(); k = 0;}
      point[0] = i * xscale;
      point[1] = j * yscale;
      point[2] = *p * zscale;
      v3d((double *)point);
    }
  }
  endpoint();
}
     
      

void display_bat_spring(vector *bat_position, vector *bat_node_posit)
{
  int current_color = getcolor();
  color(MAGENTA);
  bgnline(); v3d((double *)bat_position); v3d((double *)bat_node_posit); endline();
  color(current_color);
}
 
 
void display_bat_position(vector *bat_position, double size)
{
  move(bat_position->x-size, bat_position->y, bat_position->z);
  draw(bat_position->x+size, bat_position->y, bat_position->z);
  move(bat_position->x, bat_position->y-size, bat_position->z);
  draw(bat_position->x, bat_position->y+size, bat_position->z);
  move(bat_position->x, bat_position->y, bat_position->z-size);
  draw(bat_position->x, bat_position->y, bat_position->z+size);
}



void draw_elements_flat_shading(hybroid *h)
{
   int l, j, k;
   int numelms = h->numelems;
   vector norm;
   float fnorm[3];
   elem *elmi, *elms = h->elems;
   for (l = 0; l < numelms; l++) {
     elmi = &elms[l];
      for (j = 0; j < elmi->nomnodes; j++) {
	calc_normal(&elmi->vertices[(j-1 < 0) ? elmi->nomnodes-1 : j-1]->posit,
                    &elmi->vertices[j]->posit,
                    &elmi->vertices[(j+1 > elmi->nomnodes-1) ? 0 : j+1]->posit,
		    &norm);
        vinc(norm, elmi->vertices[j]->average_normal);
      }
    }

   for (l = 0; l < numelms; l++) {
     elmi = &elms[l];
      bgnpolygon();
      for (j = 0; j < elmi->nomnodes; j++) {
	vscale(1.0 / elmi->vertices[j]->max_elem_num, 
               elmi->vertices[j]->average_normal, norm);
 
        fnorm[0] = (float)norm.x; 
        fnorm[1] = (float)norm.y; 
        fnorm[2] = (float)norm.z;

        n3f(fnorm);
        v3d((double *)&elmi->vertices[j]->posit);
       }
      endpolygon();
    }
   
}